'''
Created on Aug 9, 2013

@author: csiebern
'''
import T3D
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.axes3d import Axes3D  # @UnresolvedImport
import numpy as np

dsn = "CE498"
ProjPath = "/Volumes/APEX_SSD/CE498"

#Create project object
CE498 = T3D.T3DProj(dsn,ProjPath)

#Create an observation array for the Left Sensor Strip 1
obsClip1Lt = CE498.LtLiDAR.getOBS(CE498.clips[0], start=0, count=3000)
obsClip1Rt = CE498.RtLiDAR.getOBS(CE498.clips[0], start=0, count=3000)

#Calculate the forward projection to ECEF using Current Sensor parameters
ecefClip1Lt = T3D.tfmECEF(obsClip1Lt, CE498.LtLiDAR)
LX1 = ecefClip1Lt[0,:]
LX2 = ecefClip1Lt[1,:]
LX3 = ecefClip1Lt[2,:]

ecefClip1Rt = T3D.tfmECEF(obsClip1Rt, CE498.RtLiDAR)

RX1 = ecefClip1Rt[0,:]
RX2 = ecefClip1Rt[1,:]
RX3 = ecefClip1Rt[2,:]

# Plot graphs of results
fig = plt.figure()

ax = fig.add_subplot(121, projection='3d')

ax.scatter3D(LX1,LX2,LX3,c='r',marker = '.')
ax.scatter3D(RX1,RX2,RX3,c='g',marker = '.')
ax.set_xlabel('Topocentric East')
ax.set_ylabel('Topocentric North')
ax.set_zlabel('Topocentric Up')
ax.set_xlim3d(-20,20)
ax.set_ylim3d(-20,20)
ax.set_zlim3d(-20,20)
ax.set_frame_on(True)

ax = fig.add_subplot(122)
ax.scatter(LX1,LX2,c='r',marker='.')
ax.scatter(RX1,RX2,c='g',marker='.')
ax.set_xlim(-20,20)
ax.set_ylim(-20,20)
ax.set_xlabel('Topocentric E')
ax.set_ylabel('Topocentric N')

plt.show()